#include <stdio.h>

#include "serpent_bits.h"

#define ROR(x, n) (x >> n | x << (32 - n))
#define ROL(x, n) (x << n | x >> (32 - n))

#define TRANS_COLS 7
#define TRANS_ROWS 128
const int8_t TRANS[TRANS_ROWS][TRANS_COLS] = {
	{ 16, 52, 56, 70, 83, 94,105}, { 72,114,125, -1, -1, -1, -1},
	{  2,  9, 15, 30, 76, 84,126}, { 36, 90,103, -1, -1, -1, -1},
	{ 20, 56, 60, 74, 87, 98,109}, {  1, 76,118, -1, -1, -1, -1},
	{  2,  6, 13, 19, 34, 80, 88}, { 40, 94,107, -1, -1, -1, -1},
	{ 24, 60, 64, 78, 91,102,113}, {  5, 80,122, -1, -1, -1, -1},
	{  6, 10, 17, 23, 38, 84, 92}, { 44, 98,111, -1, -1, -1, -1},
	{ 28, 64, 68, 82, 95,106,117}, {  9, 84,126, -1, -1, -1, -1},
	{ 10, 14, 21, 27, 42, 88, 96}, { 48,102,115, -1, -1, -1, -1},
	{ 32, 68, 72, 86, 99,110,121}, {  2, 13, 88, -1, -1, -1, -1},
	{ 14, 18, 25, 31, 46, 92,100}, { 52,106,119, -1, -1, -1, -1},
	{ 36, 72, 76, 90,103,114,125}, {  6, 17, 92, -1, -1, -1, -1},
	{ 18, 22, 29, 35, 50, 96,104}, { 56,110,123, -1, -1, -1, -1},
	{  1, 40, 76, 80, 94,107,118}, { 10, 21, 96, -1, -1, -1, -1},
	{ 22, 26, 33, 39, 54,100,108}, { 60,114,127, -1, -1, -1, -1},
	{  5, 44, 80, 84, 98,111,122}, { 14, 25,100, -1, -1, -1, -1},
	{ 26, 30, 37, 43, 58,104,112}, {  3,118, -1, -1, -1, -1, -1},
	{  9, 48, 84, 88,102,115,126}, { 18, 29,104, -1, -1, -1, -1},
	{ 30, 34, 41, 47, 62,108,116}, {  7,122, -1, -1, -1, -1, -1},
	{  2, 13, 52, 88, 92,106,119}, { 22, 33,108, -1, -1, -1, -1},
	{ 34, 38, 45, 51, 66,112,120}, { 11,126, -1, -1, -1, -1, -1},
	{  6, 17, 56, 92, 96,110,123}, { 26, 37,112, -1, -1, -1, -1},
	{ 38, 42, 49, 55, 70,116,124}, {  2, 15, 76, -1, -1, -1, -1},
	{ 10, 21, 60, 96,100,114,127}, { 30, 41,116, -1, -1, -1, -1},
	{  0, 42, 46, 53, 59, 74,120}, {  6, 19, 80, -1, -1, -1, -1},
	{  3, 14, 25,100,104,118, -1}, { 34, 45,120, -1, -1, -1, -1},
	{  4, 46, 50, 57, 63, 78,124}, { 10, 23, 84, -1, -1, -1, -1},
	{  7, 18, 29,104,108,122, -1}, { 38, 49,124, -1, -1, -1, -1},
	{  0,  8, 50, 54, 61, 67, 82}, { 14, 27, 88, -1, -1, -1, -1},
	{ 11, 22, 33,108,112,126, -1}, {  0, 42, 53, -1, -1, -1, -1},
	{  4, 12, 54, 58, 65, 71, 86}, { 18, 31, 92, -1, -1, -1, -1},
	{  2, 15, 26, 37, 76,112,116}, {  4, 46, 57, -1, -1, -1, -1},
	{  8, 16, 58, 62, 69, 75, 90}, { 22, 35, 96, -1, -1, -1, -1},
	{  6, 19, 30, 41, 80,116,120}, {  8, 50, 61, -1, -1, -1, -1},
	{ 12, 20, 62, 66, 73, 79, 94}, { 26, 39,100, -1, -1, -1, -1},
	{ 10, 23, 34, 45, 84,120,124}, { 12, 54, 65, -1, -1, -1, -1},
	{ 16, 24, 66, 70, 77, 83, 98}, { 30, 43,104, -1, -1, -1, -1},
	{  0, 14, 27, 38, 49, 88,124}, { 16, 58, 69, -1, -1, -1, -1},
	{ 20, 28, 70, 74, 81, 87,102}, { 34, 47,108, -1, -1, -1, -1},
	{  0,  4, 18, 31, 42, 53, 92}, { 20, 62, 73, -1, -1, -1, -1},
	{ 24, 32, 74, 78, 85, 91,106}, { 38, 51,112, -1, -1, -1, -1},
	{  4,  8, 22, 35, 46, 57, 96}, { 24, 66, 77, -1, -1, -1, -1},
	{ 28, 36, 78, 82, 89, 95,110}, { 42, 55,116, -1, -1, -1, -1},
	{  8, 12, 26, 39, 50, 61,100}, { 28, 70, 81, -1, -1, -1, -1},
	{ 32, 40, 82, 86, 93, 99,114}, { 46, 59,120, -1, -1, -1, -1},
	{ 12, 16, 30, 43, 54, 65,104}, { 32, 74, 85, -1, -1, -1, -1},
	{ 36, 90,103,118, -1, -1, -1}, { 50, 63,124, -1, -1, -1, -1},
	{ 16, 20, 34, 47, 58, 69,108}, { 36, 78, 89, -1, -1, -1, -1},
	{ 40, 94,107,122, -1, -1, -1}, {  0, 54, 67, -1, -1, -1, -1},
	{ 20, 24, 38, 51, 62, 73,112}, { 40, 82, 93, -1, -1, -1, -1},
	{ 44, 98,111,126, -1, -1, -1}, {  4, 58, 71, -1, -1, -1, -1},
	{ 24, 28, 42, 55, 66, 77,116}, { 44, 86, 97, -1, -1, -1, -1},
	{  2, 48,102,115, -1, -1, -1}, {  8, 62, 75, -1, -1, -1, -1},
	{ 28, 32, 46, 59, 70, 81,120}, { 48, 90,101, -1, -1, -1, -1},
	{  6, 52,106,119, -1, -1, -1}, { 12, 66, 79, -1, -1, -1, -1},
	{ 32, 36, 50, 63, 74, 85,124}, { 52, 94,105, -1, -1, -1, -1},
	{ 10, 56,110,123, -1, -1, -1}, { 16, 70, 83, -1, -1, -1, -1},
	{  0, 36, 40, 54, 67, 78, 89}, { 56, 98,109, -1, -1, -1, -1},
	{ 14, 60,114,127, -1, -1, -1}, { 20, 74, 87, -1, -1, -1, -1},
	{  4, 40, 44, 58, 71, 82, 93}, { 60,102,113, -1, -1, -1, -1},
	{  3, 18, 72,114,118,125, -1}, { 24, 78, 91, -1, -1, -1, -1},
	{  8, 44, 48, 62, 75, 86, 97}, { 64,106,117, -1, -1, -1, -1},
	{  1,  7, 22, 76,118,122, -1}, { 28, 82, 95, -1, -1, -1, -1},
	{ 12, 48, 52, 66, 79, 90,101}, { 68,110,121, -1, -1, -1, -1},
	{  5, 11, 26, 80,122,126, -1}, { 32, 86, 99, -1, -1, -1, -1}
};
const int8_t INVTRANS[TRANS_ROWS][TRANS_COLS] = {
	{ 53, 55, 72, -1, -1, -1, -1}, {  1,  5, 20, 90, -1, -1, -1},
	{ 15,102, -1, -1, -1, -1, -1}, {  3, 31, 90, -1, -1, -1, -1},
	{ 57, 59, 76, -1, -1, -1, -1}, {  5,  9, 24, 94, -1, -1, -1},
	{ 19,106, -1, -1, -1, -1, -1}, {  7, 35, 94, -1, -1, -1, -1},
	{ 61, 63, 80, -1, -1, -1, -1}, {  9, 13, 28, 98, -1, -1, -1},
	{ 23,110, -1, -1, -1, -1, -1}, { 11, 39, 98, -1, -1, -1, -1},
	{ 65, 67, 84, -1, -1, -1, -1}, { 13, 17, 32,102, -1, -1, -1},
	{ 27,114, -1, -1, -1, -1, -1}, {  1,  3, 15, 20, 43,102, -1},
	{ 69, 71, 88, -1, -1, -1, -1}, { 17, 21, 36,106, -1, -1, -1},
	{  1, 31,118, -1, -1, -1, -1}, {  5,  7, 19, 24, 47,106, -1},
	{ 73, 75, 92, -1, -1, -1, -1}, { 21, 25, 40,110, -1, -1, -1},
	{  5, 35,122, -1, -1, -1, -1}, {  9, 11, 23, 28, 51,110, -1},
	{ 77, 79, 96, -1, -1, -1, -1}, { 25, 29, 44,114, -1, -1, -1},
	{  9, 39,126, -1, -1, -1, -1}, { 13, 15, 27, 32, 55,114, -1},
	{ 81, 83,100, -1, -1, -1, -1}, {  1, 29, 33, 48,118, -1, -1},
	{  2, 13, 43, -1, -1, -1, -1}, {  1, 17, 19, 31, 36, 59,118},
	{ 85, 87,104, -1, -1, -1, -1}, {  5, 33, 37, 52,122, -1, -1},
	{  6, 17, 47, -1, -1, -1, -1}, {  5, 21, 23, 35, 40, 63,122},
	{ 89, 91,108, -1, -1, -1, -1}, {  9, 37, 41, 56,126, -1, -1},
	{ 10, 21, 51, -1, -1, -1, -1}, {  9, 25, 27, 39, 44, 67,126},
	{ 93, 95,112, -1, -1, -1, -1}, {  2, 13, 41, 45, 60, -1, -1},
	{ 14, 25, 55, -1, -1, -1, -1}, {  2, 13, 29, 31, 43, 48, 71},
	{ 97, 99,116, -1, -1, -1, -1}, {  6, 17, 45, 49, 64, -1, -1},
	{ 18, 29, 59, -1, -1, -1, -1}, {  6, 17, 33, 35, 47, 52, 75},
	{101,103,120, -1, -1, -1, -1}, { 10, 21, 49, 53, 68, -1, -1},
	{ 22, 33, 63, -1, -1, -1, -1}, { 10, 21, 37, 39, 51, 56, 79},
	{105,107,124, -1, -1, -1, -1}, { 14, 25, 53, 57, 72, -1, -1},
	{ 26, 37, 67, -1, -1, -1, -1}, { 14, 25, 41, 43, 55, 60, 83},
	{  0,109,111, -1, -1, -1, -1}, { 18, 29, 57, 61, 76, -1, -1},
	{ 30, 41, 71, -1, -1, -1, -1}, { 18, 29, 45, 47, 59, 64, 87},
	{  4,113,115, -1, -1, -1, -1}, { 22, 33, 61, 65, 80, -1, -1},
	{ 34, 45, 75, -1, -1, -1, -1}, { 22, 33, 49, 51, 63, 68, 91},
	{  8,117,119, -1, -1, -1, -1}, { 26, 37, 65, 69, 84, -1, -1},
	{ 38, 49, 79, -1, -1, -1, -1}, { 26, 37, 53, 55, 67, 72, 95},
	{ 12,121,123, -1, -1, -1, -1}, { 30, 41, 69, 73, 88, -1, -1},
	{ 42, 53, 83, -1, -1, -1, -1}, { 30, 41, 57, 59, 71, 76, 99},
	{ 16,125,127, -1, -1, -1, -1}, { 34, 45, 73, 77, 92, -1, -1},
	{ 46, 57, 87, -1, -1, -1, -1}, { 34, 45, 61, 63, 75, 80,103},
	{  1,  3, 20, -1, -1, -1, -1}, { 38, 49, 77, 81, 96, -1, -1},
	{ 50, 61, 91, -1, -1, -1, -1}, { 38, 49, 65, 67, 79, 84,107},
	{  5,  7, 24, -1, -1, -1, -1}, { 42, 53, 81, 85,100, -1, -1},
	{ 54, 65, 95, -1, -1, -1, -1}, { 42, 53, 69, 71, 83, 88,111},
	{  9, 11, 28, -1, -1, -1, -1}, { 46, 57, 85, 89,104, -1, -1},
	{ 58, 69, 99, -1, -1, -1, -1}, { 46, 57, 73, 75, 87, 92,115},
	{ 13, 15, 32, -1, -1, -1, -1}, { 50, 61, 89, 93,108, -1, -1},
	{ 62, 73,103, -1, -1, -1, -1}, { 50, 61, 77, 79, 91, 96,119},
	{ 17, 19, 36, -1, -1, -1, -1}, { 54, 65, 93, 97,112, -1, -1},
	{ 66, 77,107, -1, -1, -1, -1}, { 54, 65, 81, 83, 95,100,123},
	{ 21, 23, 40, -1, -1, -1, -1}, { 58, 69, 97,101,116, -1, -1},
	{ 70, 81,111, -1, -1, -1, -1}, { 58, 69, 85, 87, 99,104,127},
	{ 25, 27, 44, -1, -1, -1, -1}, { 62, 73,101,105,120, -1, -1},
	{ 74, 85,115, -1, -1, -1, -1}, {  3, 62, 73, 89, 91,103,108},
	{ 29, 31, 48, -1, -1, -1, -1}, { 66, 77,105,109,124, -1, -1},
	{ 78, 89,119, -1, -1, -1, -1}, {  7, 66, 77, 93, 95,107,112},
	{ 33, 35, 52, -1, -1, -1, -1}, {  0, 70, 81,109,113, -1, -1},
	{ 82, 93,123, -1, -1, -1, -1}, { 11, 70, 81, 97, 99,111,116},
	{ 37, 39, 56, -1, -1, -1, -1}, {  4, 74, 85,113,117, -1, -1},
	{ 86, 97,127, -1, -1, -1, -1}, { 15, 74, 85,101,103,115,120},
	{ 41, 43, 60, -1, -1, -1, -1}, {  8, 78, 89,117,121, -1, -1},
	{  3, 90, -1, -1, -1, -1, -1}, { 19, 78, 89,105,107,119,124},
	{ 45, 47, 64, -1, -1, -1, -1}, { 12, 82, 93,121,125, -1, -1},
	{  7, 94, -1, -1, -1, -1, -1}, {  0, 23, 82, 93,109,111,123},
	{ 49, 51, 68, -1, -1, -1, -1}, {  1, 16, 86, 97,125, -1, -1},
	{ 11, 98, -1, -1, -1, -1, -1}, {  4, 27, 86, 97,113,115,127}
};

const uint8_t Sbox[8][16] = {
	{ 3, 8,15, 1,10, 6, 5,11,14,13, 4, 2, 7, 0, 9,12},
	{15,12, 2, 7, 9, 0, 5,10, 1,11,14, 8, 6,13, 3, 4},
	{ 8, 6, 7, 9, 3,12,10,15,13, 1,14, 4, 0,11, 5, 2},
	{ 0,15,11, 8,12, 9, 6, 3,13, 1, 2, 4,10, 7, 5,14},
	{ 1,15, 8, 3,12, 0,11, 6, 2, 5, 4,10, 9,14, 7,13},
	{15, 5, 2,11, 4,10, 9,12, 0, 3,14, 8,13, 6, 7, 1},
	{ 7, 2,12, 5, 8, 4, 6,11,14, 9, 1,15,13, 3,10, 0},
	{ 1,13,15, 0,14, 8, 2,11, 7, 4,12,10, 9, 3, 5, 6}
};
const uint8_t Sboxinv[8][16] = {
	{13, 3,11, 0,10, 6, 5,12, 1,14, 4, 7,15, 9, 8, 2},
	{ 5, 8, 2,14,15, 6,12, 3,11, 4, 7, 9, 1,13,10, 0},
	{12, 9,15, 4,11,14, 1, 2, 0, 3, 6,13, 5, 8,10, 7},
	{ 0, 9,10, 7,11,14, 6,13, 3, 5,12, 2, 4, 8,15, 1},
	{ 5, 0, 8, 3,10, 9, 7,14, 2,12,11, 6, 4,15,13, 1},
	{ 8,15, 2, 9, 4, 1,13,14,11, 6, 5, 3, 7,12,10, 0},
	{15,10, 1,13, 5, 3, 6, 0, 4, 9,14, 7, 2,12, 8,11},
	{ 3, 0, 6,13, 9,14,15, 8, 5,12,11, 7,10, 1, 4, 2}
};

void
perm(const uint32_t in[4], uint32_t out[4])
{
	for (uint8_t i = 0; i < 4; i++) out[i] = 0;
	for (uint8_t i = 0; i < 128; i++) {
		uint8_t j = i / 32 + (i % 32) * 4;
		if (in[i/32] & (1 << i%32))
			out[j/32] |= 1 << j%32;
	}
}

void
perm_inv(const uint32_t in[4], uint32_t out[4])
{
	for (uint8_t i = 0; i < 4; i++) out[i] = 0;
	for (uint8_t i = 0; i < 128; i++) {
		uint8_t j = i / 4 + (i % 4) * 32;
		if (in[i/32] & (1 << i%32))
			out[j/32] |= 1 << j%32;
	}
}

void _t_hat(const int8_t matrix[TRANS_ROWS][TRANS_COLS],
    const uint32_t in[4], uint32_t out[4])
{
	for (uint8_t i = 0; i < 4; i++) out[i] = 0;
	for (uint8_t i = 0; i < TRANS_ROWS; i++) {
		uint8_t b = 0;
		for (uint8_t j = 0; j < TRANS_COLS; j++) {
			if (matrix[i][j] == -1) continue;
			uint8_t k = matrix[i][j];
			if (in[k/32] & (1 << k%32))
				/* logical XOR */
				b = b != 1;
		}
		out[i/32] |= b << i%32;
	}
}

void t_hat(const uint32_t in[4], uint32_t out[4])
{
	_t_hat(TRANS, in, out);
}

void t_hat_inv(const uint32_t in[4], uint32_t out[4])
{
	_t_hat(INVTRANS, in, out);
}

void _s_hat(const uint8_t perm[16], const uint32_t in[4], uint32_t out[4])
{
	for (uint8_t i = 0; i < 4; i++) out[i] = 0;
	for (uint8_t i = 0; i < 32; i++) {
		uint8_t x = (in[i/8] >> (i%8 * 4)) & 0xf;
		out[i/8] |= perm[x] << i%8 * 4;
	}
}

void s_hat(uint8_t which, const uint32_t in[4], uint32_t out[4])
{
	_s_hat(Sbox[which], in, out);
}

void s_hat_inv(uint8_t which, const uint32_t in[4], uint32_t out[4])
{
	_s_hat(Sboxinv[which], in, out);
}

void trans(const uint32_t in[4], uint32_t out[4])
{
	out[0] = ROL(in[0], 13);
	out[2] = ROL(in[2], 3);
	out[1] = in[1] ^ out[0] ^ out[2];
	out[3] = in[3] ^ out[2] ^ (out[0] << 3);
	out[1] = ROL(out[1], 1);
	out[3] = ROL(out[3], 7);
	out[0] ^= out[1] ^ out[3];
	out[2] ^= out[3] ^ (out[1] << 7);
	out[0] = ROL(out[0], 5);
	out[2] = ROL(out[2], 22);
}

void trans_inv(const uint32_t in[4], uint32_t out[4])
{
	out[2] = ROR(in[2], 22);
	out[0] = ROR(in[0], 5);
	out[2] ^= in[3] ^ (in[1] << 7);
	out[0] ^= in[1] ^ in[3];
	out[3] = ROR(in[3], 7);
	out[1] = ROR(in[1], 1);
	out[3] ^= out[2] ^ (out[0] << 3);
	out[1] ^= out[0] ^ out[2];
	out[2] = ROR(out[2], 3);
	out[0] = ROR(out[0], 13);
}

void dump32(const uint32_t *b, size_t count)
{
	for (size_t i = 0; i < count; i++) {
		printf("%08x ", b[i]);
	}
	printf("\n");
}

void sbox(uint8_t which, const uint32_t in[4], uint32_t out[4])
{
	uint32_t a[4];
	uint32_t b[4];
	uint32_t c[4];
	uint32_t d[4];

	perm(in, a);
	s_hat(which, a, b);
	t_hat(b, c);
	perm_inv(c, d);
	trans_inv(d, out);
}

void sbox_inv(uint8_t which, const uint32_t in[4], uint32_t out[4])
{
	uint32_t a[4];
	uint32_t b[4];
	uint32_t c[4];
	uint32_t d[4];

	trans(in, a);
	perm(a, b);
	t_hat_inv(b, c);
	s_hat_inv(which, c, d);
	perm_inv(d, out);
}
